/*
*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "driver/gpio.h"
#include "driver/ledc.h"
#include "esp_err.h"


#define GPIO_OUTPUT_IO_0       19
#define GPIO_OUTPUT_PIN_SEL  (1ULL<<GPIO_OUTPUT_IO_0)

#define SERVO_LS_TIMER          LEDC_TIMER_1
#define SERVO_LS_MODE           LEDC_LOW_SPEED_MODE

#define MOTOR_LS_TIMER        LEDC_TIMER_2
#define MOTOR_LS_MODE         LEDC_LOW_SPEED_MODE

#define SERVO_LS_CH0_GPIO       (3)
#define SERVO_LS_CH0_CHANNEL    LEDC_CHANNEL_0
#define MOTOR_LS_CH1_GPIO       (4)
#define MOTOR_LS_CH1_CHANNEL    LEDC_CHANNEL_1
#define MOTOR_LS_CH2_GPIO       (5)
#define MOTOR_LS_CH2_CHANNEL    LEDC_CHANNEL_2

#define PWM_TEST_CH_NUM       (3)
#define PWM_TEST_DUTY         (4000)
#define PWM_TEST_FADE_TIME    (1000)

#define SERVO_MIN_PULSEWIDTH 1000 //Minimum pulse width in microsecond
#define SERVO_MAX_PULSEWIDTH 2000 //Maximum pulse width in microsecond
#define SERVO_MAX_DEGREE 90 //Maximum angle in degree upto which servo can rotate

void hal_gpio_init(void)
{
    gpio_config_t config = {
        .pin_bit_mask = GPIO_OUTPUT_PIN_SEL, //引脚选择
        .mode = GPIO_MODE_OUTPUT, //输出模式                                
        .pull_up_en = GPIO_PULLUP_ENABLE, //使能上拉                          
        .pull_down_en = GPIO_PULLDOWN_DISABLE, //失能下拉                      
        .intr_type = GPIO_PIN_INTR_DISABLE, //失能中断                         
    };
    gpio_config(&config); //初始化
    gpio_set_level(GPIO_OUTPUT_IO_0, 1); //设置输出为高电平

}

void gpio_blink(void *arg)
{
   hal_gpio_init();

    while(1)
	{
        gpio_set_level(GPIO_OUTPUT_IO_0, 1);
		vTaskDelay(pdMS_TO_TICKS(1000)); //FreeRTOS延时函数，延时1秒
        gpio_set_level(GPIO_OUTPUT_IO_0, 0);
		vTaskDelay(pdMS_TO_TICKS(1000));
	}
}

void servo_control(void *arg)
{
    int ch;

    ledc_timer_config_t servo_timer = {
        .duty_resolution = LEDC_TIMER_13_BIT, // resolution of PWM duty
        .freq_hz = 5000,                      // frequency of PWM signal
        .speed_mode = SERVO_LS_MODE,           // timer mode
        .timer_num = SERVO_LS_TIMER,            // timer index
        .clk_cfg = LEDC_AUTO_CLK,              // Auto select the source clock
    };

    ledc_timer_config_t motor_timer = {
        .duty_resolution = LEDC_TIMER_13_BIT, // resolution of PWM duty
        .freq_hz = 5000,                      // frequency of PWM signal
        .speed_mode = MOTOR_LS_MODE,           // timer mode
        .timer_num = MOTOR_LS_TIMER,            // timer index
        .clk_cfg = LEDC_AUTO_CLK,              // Auto select the source clock
    };

    ledc_timer_config(&servo_timer);
    ledc_timer_config(&motor_timer);

    ledc_channel_config_t pwm_channel[PWM_TEST_CH_NUM] = {

        {
            .channel    = SERVO_LS_CH0_CHANNEL,
            .duty       = 0,
            .gpio_num   = SERVO_LS_CH0_GPIO,
            .speed_mode = SERVO_LS_MODE,
            .hpoint     = 0,
            .timer_sel  = SERVO_LS_TIMER
        },
        {
            .channel    = MOTOR_LS_CH1_CHANNEL,
            .duty       = 0,
            .gpio_num   = MOTOR_LS_CH1_GPIO,
            .speed_mode = MOTOR_LS_MODE,
            .hpoint     = 0,
            .timer_sel  = MOTOR_LS_TIMER
        },
        {
            .channel    = MOTOR_LS_CH2_CHANNEL,
            .duty       = 0,
            .gpio_num   = MOTOR_LS_CH2_GPIO,
            .speed_mode = MOTOR_LS_MODE,
            .hpoint     = 0,
            .timer_sel  = MOTOR_LS_TIMER
        }
    };

    // Set LED Controller with previously prepared configuration
    for (ch = 0; ch < PWM_TEST_CH_NUM; ch++) {
        ledc_channel_config(&pwm_channel[ch]);
    }

    // Initialize fade service.
    ledc_fade_func_install(0);

    while (1) {
        //printf("1. LEDC fade up to duty = %d\n", PWM_TEST_DUTY);
        for (ch = 0; ch < PWM_TEST_CH_NUM; ch++) {
            ledc_set_fade_with_time(pwm_channel[ch].speed_mode,
                    pwm_channel[ch].channel, PWM_TEST_DUTY, PWM_TEST_FADE_TIME);
            ledc_fade_start(pwm_channel[ch].speed_mode,
                    pwm_channel[ch].channel, LEDC_FADE_NO_WAIT);
        }
        vTaskDelay(PWM_TEST_FADE_TIME / portTICK_PERIOD_MS);

       // printf("2. LEDC fade down to duty = 0\n");
        for (ch = 0; ch < PWM_TEST_CH_NUM; ch++) {
            ledc_set_fade_with_time(pwm_channel[ch].speed_mode,
                    pwm_channel[ch].channel, 0, PWM_TEST_FADE_TIME);
            ledc_fade_start(pwm_channel[ch].speed_mode,
                    pwm_channel[ch].channel, LEDC_FADE_NO_WAIT);
        }
        vTaskDelay(PWM_TEST_FADE_TIME / portTICK_PERIOD_MS);
    }

}

void app_main(void)
{

    printf("START!!!\n");

    xTaskCreate(gpio_blink, "gpio_blink", 4096, NULL, 5, NULL);

    xTaskCreate(servo_control, "servo_control", 4096, NULL, 5, NULL);

}
